#include "ADRC.h"


/*********限幅函数*********/ 
float Constrain_Float(float amt, float low, float high)
{ 
    return ((amt)<(low)?(low):((amt)>(high)?(high):(amt))); 
} 

/********* Sign 函数*********/ 
int16_t Sign_ADRC(float Input) 
{ 
    int16_t output=0;             
    if(Input>1E-6) output=1; 
    else if(Input<-1E-6) output=-1;  else output=0; 
    return output; 
}


/********* Fsg 函数*********/ 
int16_t Fsg_ADRC(float x,float d) 
{ 
    int16_t output=0; 
    output=(Sign_ADRC(x+d)-Sign_ADRC(x-d))/2; 
    return output; 
} 


/*********幂次函数*********/ 
float Fal_ADRC(float e,float alpha,float m) 
{ 
    int16 s=0;      
    float fal_output=0; 
    s=(Sign_ADRC(e+m)-Sign_ADRC(e-m))/2; 
    fal_output=e*s/(powf(m,1-alpha))+powf(ABS(e),alpha)*Sign_ADRC(e)*(1-s); 
    return fal_output; 
} 


/*********初始化参数*********/ 
void ADRC_Init(ADRC_Data *ADRC_Input) 
{ 
    ADRC_Input->r=ADRC_Unit[0]; 
    ADRC_Input->h=ADRC_Unit[1]; 
    ADRC_Input->w0=ADRC_Unit[2]; 
    ADRC_Input->b0=ADRC_Unit[3]; 
    ADRC_Input->a0=ADRC_Unit[4]; 
    ADRC_Input->a1=ADRC_Unit[5]; 
    ADRC_Input->m=ADRC_Unit[6]; 
} 


/************ADRC 跟踪微分器TD********************/ 



/**
 * @brief ADRC 跟踪微分器TD的功能实现
 * 
 * @param ADRC_Input 
 * @param expect_ADRC 
 * @return ** void 
 */
void Fhan_ADRC(ADRC_Data *ADRC_Input,float expect_ADRC)  
{ 
    float d=0,a0=0,y=0,a1=0,a2=0,a=0; 
    float x1_delta=0; 
    x1_delta=ADRC_Input->x1-expect_ADRC;  
    ADRC_Input->h0=20*ADRC_Input->h; 
    d=ADRC_Input->r*ADRC_Input->h0*ADRC_Input->h0;//d=rh0^2; 
    a0=ADRC_Input->h0*ADRC_Input->x2;//a0=h*x2 
    y=x1_delta+a0;//y=x1+a0 
    a1=sqrt(d*(d+8*ABS(y)));//a1=sqrt(d*(d+8*ABS(y))]) 
    a2=a0+Sign_ADRC(y)*(a1-d)/2;//a2=a0+sign(y)*(a1-d)/2; 
    a=(a0+y)*Fsg_ADRC(y,d)+a2*(1-Fsg_ADRC(y,d)); 
    ADRC_Input->fh=-ADRC_Input->r*(a/d)*Fsg_ADRC(a,d) -ADRC_Input->r*Sign_ADRC(a)*(1-Fsg_ADRC(a,d));  
    ADRC_Input->x1+=ADRC_Input->h*ADRC_Input->x2; 
    ADRC_Input->x2+=ADRC_Input->h*ADRC_Input->fh; 
} 




/**
 * @brief 扩张状态观测器
 * 
 * @param ADRC_Input ADRC参数结构体
 * 该函数实现状态观测器中各阶状态量的计算
 */
void LESO_ADRC(ADRC_Data *ADRC_Input) 
{ 
    ADRC_Input->e=ADRC_Input->z1-ADRC_Input->y;//状态误差 
    ADRC_Input->z1+=ADRC_Input->h*(ADRC_Input->z2-3*w0*ADRC_Input->e); 
    ADRC_Input->z2+=ADRC_Input->h*(ADRC_Input->z3-3*w0*wo*ADRC_Input->e+ADRC_Input->b0*ADRC_Input->u+ADRC_Input->Ff); 
    ADRC_Input->z3+=ADRC_Input->h*(-w0*w0*w0*ADRC_Input->e); 
} 


/**************非线性组合********************/ 
void Nolinear_Conbination_ADRC(ADRC_Data *ADRC_Input) 
{ 
    float temp_e2=0; 
    temp_e2=Constrain_Float(ADRC_Input->e2,-3000,3000); 
    ADRC_Input->u0=ADRC_Input->a1*Fal_ADRC(ADRC_Input->e1,0.5,ADRC_Input->zeta)+ADRC_Input->a2*Fal_ADRC(temp_e2,0.25,ADRC_Input->zeta); 
} 


/**************摩擦力矩 Ff**************************/ 
void Friction_ADRC(ADRC_Data *ADRC_Input) 
{ 
    float temp_g(v)=0, temp_fr=0; 
    temp_fr=powf(ADRC_Input->v,2); 
    temp_g(v)=0.2775+(0.3399-0.2775)*exp(-temp_fr); 
    z(k+1)=z(k)+h*(ADRC_Input->v-236*z(k)*ABS(ADRC_Input->v)/g(v)); 
    ADRC_Input->Ff= 236*z(k)+2.6*(z(k+1)-z(k))/h+0.002*ADRC_Input->v; 
} 

/*********************主程序**********************/ 
void ADRC_Control(ADRC_Data*ADRC_Input,float expect_ADRC,float feedback_y,float feedback_v) 
{ 
    Fhan_ADRC(ADRC_Input,expect_ADRC); 
    ADRC_Input->y=feedback_y; 
    ADRC_Input->v=feedback_v; 
    Friction_ADRC(ADRC_Input); 
    LESO_ADRC(ADRC_Input); 
    ADRC_Input->e0+=ADRC_Input->e1*ADRC_Input->h; 
    ADRC_Input->e1=ADRC_Input->x1-ADRC_Input->z1; 
    ADRC_Input->e2=ADRC_Input->x2-ADRC_Input->z2; 
    Nolinear_Conbination_ADRC(ADRC_Input); 
    ADRC_Input->u=ADRC_Input->u0 -ADRC_Input->z3+ADRC_Input->Ff; 
    ADRC_Input->u=Constrain_Float(ADRC_Input->u0,-2000,2000); 
}